#ifndef RANSAC_HPP
#define RANSAC_HPP

#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */
#include <iostream>
#include <algorithm>

template<typename DATA, typename MODEL>
class Ransac
{
public:
    MODEL model;
protected:
    DATA *data;
    size_t nData;
    size_t nSelect;
    double *error;

    double errorThreshold;
    double acceptPercentage;

    double MissRatioOnce() const
    {
        double ratio=1;
        size_t nDataHit=nData*acceptPercentage;
        for(size_t i=0;i<nSelect;i++)
        {
            ratio*=(nDataHit-i);
            ratio/=(nData-i);
        }
        return 1-ratio;
    }

    double IterateCount(double lgp=-12) const
    {
        return lgp/log10(MissRatioOnce());
    }

    Ransac()
        : data(NULL)
        , error(NULL)
        , nData(0)
        , nSelect(0)
        , errorThreshold(0)
        , acceptPercentage(1)
    {

    }

    void Release()
    {
        if(data!=NULL)
            delete data;

        if(error!=NULL)
            delete error;
    }

    virtual ~Ransac()
    {
        Release();
    }


    virtual void ComputeError(const MODEL &modelFit)
    {
        memset(error,0,nData*sizeof(*error));
    }

    virtual void ComputeModel(const size_t *indexSelect, MODEL &modelFit)
    {

    }

    size_t RecoverIndex(size_t in, const size_t *seg, size_t n)
    {
        for(size_t i=0;i<n;i++)
        {
            if(in<seg[i])
                break;
            in++;
        }
        return in;
    }

    void ChooseRandom(size_t *index)
    {
        for(size_t i=0;i<nSelect;i++)
            index[i]=rand()%(nData-i);

        for(size_t i=1;i<nSelect;i++)
        {
            index[i]=RecoverIndex(index[i],index,i);
            std::inplace_merge(index,index+i,index+i+1);
        }
    }

    size_t AcceptCount()
    {
        size_t n=0;
        for(size_t i=0;i<nData;i++)
        {
            if(error[i]<errorThreshold)
                n++;
        }
        return n;
    }
public:
    bool Fit(size_t &acceptCountMax, size_t nIterate)
    {
        size_t *indexSelect=new size_t[nSelect];

        /* initialize random seed: */
        srand (time(NULL));

        ChooseRandom(indexSelect);
        ComputeModel(indexSelect,model);
        ComputeError(model);
        acceptCountMax=AcceptCount();

//                std::cout<<"acceptCountMax="<<acceptCountMax<<'\n'<<std::flush;

        if(acceptCountMax>nData*acceptPercentage)
        {
            delete indexSelect;
            return true;
        }

      
//        std::cout<<"nIterate="<<nIterate<<'\n'<<std::flush;

        for(size_t i=0;i<nIterate;i++)
        {
            MODEL mt;
            ChooseRandom(indexSelect);
            ComputeModel(indexSelect,mt);
            ComputeError(mt);
            size_t ac=AcceptCount();

//                        std::cout<<"ac="<<ac<<'\n'<<std::flush;


            if(ac>acceptCountMax)
            {
//                                std::cout<<"acceptCountMax="<<acceptCountMax<<'\n'<<std::flush;

                acceptCountMax=ac;
                model=mt;
                if(acceptCountMax>nData*acceptPercentage)
                {
                    delete indexSelect;
                    return true;
                }
            }
        }

        delete indexSelect;
        return false;
    }

    bool Fit(size_t &acceptCountMax)
    {        
        return Fit(acceptCountMax, IterateCount());
    }

    void GetInOutFlag(bool *inFlag)
    {
        ComputeError(model);
        for(size_t i=0;i<nData;i++)
        {
            inFlag[i]=(error[i]<errorThreshold);
        }
    }

};




#endif // RANSAC_HPP
